import serial
import binascii

def makeWord(num, bigHead = False):
	if (num < 0) or (num > 65535): raise Exception('Not valid number')

	num_h = hex(num / 256)[2:]
	num_l = hex(num % 256)[2:]
	if len(num_h) == 1: num_h = '0' + num_h
	if len(num_l) == 1: num_l = '0' + num_l

	retStr = None
	if bigHead: retStr = num_h + num_l
	else: retStr = num_l + num_h

	return retStr.upper()

def parseWord(wordStr, bigHead = False):
	if len(wordStr) != 4: raise Exception('Not valid word string')
	word1 = wordStr[0:2]
	word2 = wordStr[2:4]
	word_h = '0'
	word_l = '0'
	if bigHead:
		word_h = word1
		word_l = word2
	else:
		word_l = word1
		word_h = word2
	return int(word_h, 16) * 256 + int(word_l, 16)

def makeByte(num):
	if (num < 0) or (num > 255): raise Exception('Not valid number')
	num_str = hex(num)[2:]
	if len(num_str) == 1: num_str = '0' + num_str
	return num_str.upper()

def parseByte(byte):
	# print(len(byte))
	if len(byte) != 2: raise Exception('Not valid byte string')
	return int(byte, 16)

def makeCommandPackage(cmd = '0000', data = '', packaged = True):
	prefix = 'AA55'
	sid = '00'
	did = '00'

	if type(cmd) == int: cmd = makeWord(cmd, True)

	data_len = len(data)
	if data_len % 2 != 0: raise Exception("Not valid data length")
	data_len = data_len / 2
	data = data + '00000000000000000000000000000000'[data_len*2:]

	pkgData = prefix + sid + did + cmd + makeWord(data_len) + data

	pkgData = pkgData + calcChkSum(pkgData)
	pkgDataPackaged = binascii.unhexlify(pkgData)
	return pkgDataPackaged, pkgData

def calcChkSum(data):
	_len = len(data)
	sum = 0
	for i in range(0, (_len)/2):
		_byte = data[i*2:i*2 + 2]
		_byte = int(_byte, 16)
		sum += _byte

	sum = hex(sum)
	sum = sum[2:]
	sum = sum[max(0,len(sum) - 4):]
	sum = int(sum, 16)

	return makeWord(sum)

def checkSum(data):
	_len = len(data)
	# Avoid large checking
	if _len > 1000: return True
	str_data = data[0:_len - 4]
	str_chk = data[_len - 4:_len]

	sum = calcChkSum(str_data)

	return sum == str_chk.upper()

def getConnection(
		port = "/dev/tty.SLAB_USBtoUART",
		baudrate = 115200,
		timeout = None):
	
	t = serial.Serial(port, baudrate, timeout = timeout)
	print(t.portstr)

	n = t.write(binascii.unhexlify('55AA000001000000000000000000000000000000000000000001'))
	s = t.read(26)
	print(s)

	conn_success =  s.encode('hex') == 'aa55010001000200000000000000000000000000000000000301'

	if (conn_success) :
		return t
	else :
		return None